#pragma once
#ifndef _COMMON_INCLUDE_H_
#define _COMMON_INCLUDE_H_

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

#include <deque>
#include <mutex>
#include<stack>
#include <unordered_map>
#include<chrono>
#include<thread>
#include<condition_variable>
#include <atomic>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h> 
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include<pcl/common/angles.h>
#include <pcl/registration/icp.h>

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>

#include<tf/transform_datatypes.h>

// for Sophus
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
typedef Sophus::SE3d SE3;
typedef Sophus::SO3d SO3;

#include"opencv2/opencv.hpp"

using namespace std;
using PointType = pcl::PointXYZI;

struct LoopFactor{
    int cur_idx = -1;
    int loop_idx = -1;
    gtsam::Pose3 T_cur_loopframe;
    gtsam::noiseModel::Diagonal::shared_ptr constraintNoise;
};

#endif
